package zpp_nape.constraint;
import zpp_nape.Config;
import zpp_nape.Const;
import zpp_nape.util.Flags;
import DummyNapeMain;
import zpp_nape.util.Math;
import zpp_nape.util.Names;
import zpp_nape.util.Pool;
import zpp_nape.util.Lists;
import zpp_nape.shape.Circle;
import zpp_nape.shape.Edge;
import zpp_nape.space.Space;
import zpp_nape.shape.Polygon;
import zpp_nape.shape.Shape;
import zpp_nape.phys.FeatureMix;
import zpp_nape.phys.FluidProperties;
import zpp_nape.phys.Material;
import zpp_nape.phys.Body;
import zpp_nape.geom.AABB;
import zpp_nape.geom.Distance;
import zpp_nape.geom.Mat23;
import zpp_nape.geom.Vec2;
import zpp_nape.geom.VecMath;
import zpp_nape.geom.Collide;
import zpp_nape.dynamics.Arbiter;
import zpp_nape.dynamics.Contact;
import zpp_nape.dynamics.InteractionFilter;
import zpp_nape.constraint.Constraint;
import zpp_nape.dynamics.SpaceArbiterList;
import zpp_nape.constraint.Matrices;
import zpp_nape.constraint.PivotJoint;
import nape.util.Flags;
import nape.util.Lists;
import nape.util.Debug;
import nape.space.Space;
import nape.shape.Circle;
import nape.shape.Polygon;
import nape.shape.Edge;
import nape.shape.Shape;
import nape.phys.FluidProperties;
import nape.phys.Material;
import nape.phys.Body;
import nape.geom.AABB;
import nape.geom.Geom;
import nape.geom.Mat23;
import nape.geom.Vec2;
import nape.dynamics.Contact;
import nape.dynamics.Arbiter;
import nape.dynamics.InteractionFilter;
#if swc@:keep#end class ZPP_WeldJoint extends ZPP_Constraint{
    public var b1:ZPP_Body;
    public var a1localx:Float;
    public var a1localy:Float;
    public var a1relx:Float;
    public var a1rely:Float;
    public var b2:ZPP_Body;
    public var a2localx:Float;
    public var a2localy:Float;
    public var a2relx:Float;
    public var a2rely:Float;
    public var phase:Float;
    public var kMassa:Float;
    public var kMassb:Float;
    public var kMassc:Float;
    public var kMassd:Float;
    public var kMasse:Float;
    public var kMassf:Float;
    public var jAccx:Float;
    public var jAccy:Float;
    public var jAccz:Float;
    public var jMax:Float;
    public var frequency:Float;
    public var damping:Float;
    public var gamma:Float;
    public var biasx:Float;
    public var biasy:Float;
    public var biasz:Float;
    public function new(){
        super();
        phase=0;
        {
            {
                jAccx=0;
                jAccy=0;
                {
                    #if NAPE_ASSERT var res={
                        !({
                            #if flash9 var ret:Bool=untyped __global__["isNaN"](jAccx);
                            ret;
                            #else Math.isNaN(jAccx);
                            #end
                        });
                    };
                    if(!res)throw "assert("+"!assert_isNaN(jAccx)"+") :: "+("vec_set(in n: "+"jAcc"+",in x: "+"0"+",in y: "+"0"+")");
                    #end
                };
                {
                    #if NAPE_ASSERT var res={
                        !({
                            #if flash9 var ret:Bool=untyped __global__["isNaN"](jAccy);
                            ret;
                            #else Math.isNaN(jAccy);
                            #end
                        });
                    };
                    if(!res)throw "assert("+"!assert_isNaN(jAccy)"+") :: "+("vec_set(in n: "+"jAcc"+",in x: "+"0"+",in y: "+"0"+")");
                    #end
                };
            };
            jAccz=0;
        };
        frequency=damping=0;
        jMax=ZPP_Const.POSINF;
    }
    public override function preStep(dt:Float){
        {
            a1relx=(b1.axisy*a1localx-b1.axisx*a1localy);
            a1rely=(a1localx*b1.axisx+a1localy*b1.axisy);
        };
        {
            a2relx=(b2.axisy*a2localx-b2.axisx*a2localy);
            a2rely=(a2localx*b2.axisx+a2localy*b2.axisy);
        };
        {
            var msum=b1.smass+b2.smass;
            kMassa=kMassd=msum;
            kMassb=kMassc=kMasse=kMassf=0;
            if(b1.sinertia!=0){
                kMassa+=a1rely*a1rely*b1.sinertia;
                kMassb-=a1relx*a1rely*b1.sinertia;
                kMassc-=a1rely*b1.sinertia;
                kMassd+=a1relx*a1relx*b1.sinertia;
                kMasse+=a1relx*b1.sinertia;
                kMassf+=b1.sinertia;
            }
            if(b2.sinertia!=0){
                kMassa+=a2rely*a2rely*b2.sinertia;
                kMassb-=a2relx*a2rely*b2.sinertia;
                kMassc-=a2rely*b1.sinertia;
                kMassd+=a2relx*a2relx*b2.sinertia;
                kMasse+=a2relx*b2.sinertia;
                kMassf+=b2.sinertia;
            }
        };
        {
            var det=(kMassa*(1-kMasse*kMasse)+kMassd*(1-kMassc*kMassc)+kMassf*(1-kMassb*kMassb)+2*kMassb*kMassc*kMasse);
            if(det<ZPP_Const.EPSILON){
                kMassa=0;
                kMassb=0;
                kMassc=0;
                kMassd=0;
                kMasse=0;
                kMassf=0;
            };
            else{
                det=1/det;
                var A=det*(kMassd*kMassf-kMasse*kMasse);
                var B=det*(kMasse*kMassc-kMassb*kMassf);
                var C=det*(kMassb*kMasse-kMassc*kMassd);
                var D=det*(kMassa*kMassf-kMassc*kMassc);
                var E=det*(kMassb*kMassc-kMassa*kMasse);
                var F=det*(kMassa*kMassd-kMassb*kMassb);
                {
                    kMassa=A;
                    kMassb=B;
                    kMassc=C;
                    kMassd=D;
                    kMasse=E;
                    kMassf=F;
                };
            }
        };
        if(frequency!=0){
            var biasCoef;
            {
                var X=({
                    var omega=2*Math.PI*frequency;
                    gamma=1/(omega*dt*(2*damping+omega*dt));
                    var ig=1/(1+gamma);
                    biasCoef=omega*omega*gamma;
                    gamma*=ig;
                    ig;
                });
                kMassa*=X;
                kMassb*=X;
                kMassc*=X;
                kMassd*=X;
                kMasse*=X;
                kMassf*=X;
            };
            {
                biasx=(b2.posx+a2relx)-(b1.posx+a1relx);
                biasy=(b2.posy+a2rely)-(b1.posy+a1rely);
                biasz=b2.rot-b1.rot-phase;
                if(false){
                    biasx+=b2.bias_velx-b1.bias_velx;
                    biasy+=b2.bias_vely-b1.bias_vely;
                    biasz+=b2.bias_angvel-b1.bias_angvel;
                }
            };
            {
                var t=(-dt*biasCoef);
                {
                    var t=(t);
                    {
                        #if NAPE_ASSERT var res={
                            !({
                                #if flash9 var ret:Bool=untyped __global__["isNaN"](t);
                                ret;
                                #else Math.isNaN(t);
                                #end
                            });
                        };
                        if(!res)throw "assert("+"!assert_isNaN(t)"+") :: "+("vec_muleq(in a: "+"bias"+",in s: "+"t"+")");
                        #end
                    };
                    biasx*=t;
                    biasy*=t;
                };
                biasz*=t;
            };
        }
        else{
            {
                {
                    biasx=0;
                    biasy=0;
                    {
                        #if NAPE_ASSERT var res={
                            !({
                                #if flash9 var ret:Bool=untyped __global__["isNaN"](biasx);
                                ret;
                                #else Math.isNaN(biasx);
                                #end
                            });
                        };
                        if(!res)throw "assert("+"!assert_isNaN(biasx)"+") :: "+("vec_set(in n: "+"bias"+",in x: "+"0"+",in y: "+"0"+")");
                        #end
                    };
                    {
                        #if NAPE_ASSERT var res={
                            !({
                                #if flash9 var ret:Bool=untyped __global__["isNaN"](biasy);
                                ret;
                                #else Math.isNaN(biasy);
                                #end
                            });
                        };
                        if(!res)throw "assert("+"!assert_isNaN(biasy)"+") :: "+("vec_set(in n: "+"bias"+",in x: "+"0"+",in y: "+"0"+")");
                        #end
                    };
                };
                biasz=0;
            };
            gamma=0;
        }
        {
            {
                var t=(b1.imass);
                {
                    #if NAPE_ASSERT var res={
                        !({
                            #if flash9 var ret:Bool=untyped __global__["isNaN"](t);
                            ret;
                            #else Math.isNaN(t);
                            #end
                        });
                    };
                    if(!res)throw "assert("+"!assert_isNaN(t)"+") :: "+("vec_subeq(in a: "+"b1.vel"+",in b: "+"jAcc"+",in s: "+"b1.imass"+")");
                    #end
                };
                b1.velx-=jAccx*t;
                b1.vely-=jAccy*t;
            };
            {
                var t=(b2.imass);
                {
                    #if NAPE_ASSERT var res={
                        !({
                            #if flash9 var ret:Bool=untyped __global__["isNaN"](t);
                            ret;
                            #else Math.isNaN(t);
                            #end
                        });
                    };
                    if(!res)throw "assert("+"!assert_isNaN(t)"+") :: "+("vec_addeq(in a: "+"b2.vel"+",in b: "+"jAcc"+",in s: "+"b2.imass"+")");
                    #end
                };
                b2.velx+=jAccx*t;
                b2.vely+=jAccy*t;
            };
            b1.angvel-=((jAccy*a1relx-jAccx*a1rely)+jAccz)*b1.iinertia;
            b2.angvel+=((jAccy*a2relx-jAccx*a2rely)+jAccz)*b2.iinertia;
        };
        return false;
    }
    public override function applyImpulse(){
        var Ex:Float;
        var Ey:Float;
        var Ez:Float;
        {
            Ex=(b2.velx-a2rely*b2.angvel)-(b1.velx-a1rely*b1.angvel);
            Ey=(b2.vely+a2relx*b2.angvel)-(b1.vely+a1relx*b1.angvel);
            Ez=b2.angvel-b1.angvel;
        };
        var Jx:Float;
        var Jy:Float;
        var Jz:Float;
        {
            {
                Jx=biasx-Ex;
                Jy=biasy-Ey;
            };
            Jz=biasz-Ez;
        };
        {
            var X=kMassa*Jx+kMassb*Jy+kMassc*Jz;
            var Y=kMassb*Jx+kMassd*Jy+kMasse*Jz;
            Jz=kMassc*Jx+kMasse*Jy+kMassf*Jz;
            {
                Jx=X;
                Jy=Y;
                {
                    #if NAPE_ASSERT var res={
                        !({
                            #if flash9 var ret:Bool=untyped __global__["isNaN"](Jx);
                            ret;
                            #else Math.isNaN(Jx);
                            #end
                        });
                    };
                    if(!res)throw "assert("+"!assert_isNaN(Jx)"+") :: "+("vec_set(in n: "+"J"+",in x: "+"X"+",in y: "+"Y"+")");
                    #end
                };
                {
                    #if NAPE_ASSERT var res={
                        !({
                            #if flash9 var ret:Bool=untyped __global__["isNaN"](Jy);
                            ret;
                            #else Math.isNaN(Jy);
                            #end
                        });
                    };
                    if(!res)throw "assert("+"!assert_isNaN(Jy)"+") :: "+("vec_set(in n: "+"J"+",in x: "+"X"+",in y: "+"Y"+")");
                    #end
                };
            };
        };
        {
            var t=(gamma);
            {
                var t=(t);
                {
                    #if NAPE_ASSERT var res={
                        !({
                            #if flash9 var ret:Bool=untyped __global__["isNaN"](t);
                            ret;
                            #else Math.isNaN(t);
                            #end
                        });
                    };
                    if(!res)throw "assert("+"!assert_isNaN(t)"+") :: "+("vec_subeq(in a: "+"J"+",in b: "+"jAcc"+",in s: "+"t"+")");
                    #end
                };
                Jx-=jAccx*t;
                Jy-=jAccy*t;
            };
            Jz-=jAccz*t;
        };
        {
            var jOldx:Float;
            var jOldy:Float;
            var jOldz:Float;
            {
                {
                    jOldx=jAccx;
                    jOldy=jAccy;
                    {
                        #if NAPE_ASSERT var res={
                            !({
                                #if flash9 var ret:Bool=untyped __global__["isNaN"](jOldx);
                                ret;
                                #else Math.isNaN(jOldx);
                                #end
                            });
                        };
                        if(!res)throw "assert("+"!assert_isNaN(jOldx)"+") :: "+("vec_set(in n: "+"jOld"+",in x: "+"jAccx"+",in y: "+"jAccy"+")");
                        #end
                    };
                    {
                        #if NAPE_ASSERT var res={
                            !({
                                #if flash9 var ret:Bool=untyped __global__["isNaN"](jOldy);
                                ret;
                                #else Math.isNaN(jOldy);
                                #end
                            });
                        };
                        if(!res)throw "assert("+"!assert_isNaN(jOldy)"+") :: "+("vec_set(in n: "+"jOld"+",in x: "+"jAccx"+",in y: "+"jAccy"+")");
                        #end
                    };
                };
                jOldz=jAccz;
            };
            {
                var t=(1);
                {
                    var t=(t);
                    {
                        #if NAPE_ASSERT var res={
                            !({
                                #if flash9 var ret:Bool=untyped __global__["isNaN"](t);
                                ret;
                                #else Math.isNaN(t);
                                #end
                            });
                        };
                        if(!res)throw "assert("+"!assert_isNaN(t)"+") :: "+("vec_addeq(in a: "+"jAcc"+",in b: "+"J"+",in s: "+"t"+")");
                        #end
                    };
                    jAccx+=Jx*t;
                    jAccy+=Jy*t;
                };
                jAccz+=Jz*t;
            };
            {
                var t=(jMax);
                {
                    #if NAPE_ASSERT var res={
                        !({
                            #if flash9 var ret:Bool=untyped __global__["isNaN"](t);
                            ret;
                            #else Math.isNaN(t);
                            #end
                        });
                    };
                    if(!res)throw "assert("+"!assert_isNaN(t)"+") :: "+("vec3_clamp(in n: "+"jAcc"+", in m: "+"jMax"+")");
                    #end
                };
                var ls=((jAccx*jAccx+jAccy*jAccy)+jAccz*jAccz);
                if(ls>t*t){
                    {
                        #if NAPE_ASSERT var res={
                            ls!=0.0;
                        };
                        if(!res)throw "assert("+"ls!=0.0"+") :: "+("vec3_clamp(in n: "+"jAcc"+", in m: "+"jMax"+")");
                        #end
                    };
                    {
                        var t=(t*ZPP_Math.invsqrt(ls));
                        {
                            var t=(t);
                            {
                                #if NAPE_ASSERT var res={
                                    !({
                                        #if flash9 var ret:Bool=untyped __global__["isNaN"](t);
                                        ret;
                                        #else Math.isNaN(t);
                                        #end
                                    });
                                };
                                if(!res)throw "assert("+"!assert_isNaN(t)"+") :: "+("vec_muleq(in a: "+"jAcc"+",in s: "+"t"+")");
                                #end
                            };
                            jAccx*=t;
                            jAccy*=t;
                        };
                        jAccz*=t;
                    };
                }
            };
            {
                {
                    Jx=jAccx-jOldx;
                    Jy=jAccy-jOldy;
                };
                Jz=jAccz-jOldz;
            };
        };
        {
            {
                var t=(b1.imass);
                {
                    #if NAPE_ASSERT var res={
                        !({
                            #if flash9 var ret:Bool=untyped __global__["isNaN"](t);
                            ret;
                            #else Math.isNaN(t);
                            #end
                        });
                    };
                    if(!res)throw "assert("+"!assert_isNaN(t)"+") :: "+("vec_subeq(in a: "+"b1.vel"+",in b: "+"J"+",in s: "+"b1.imass"+")");
                    #end
                };
                b1.velx-=Jx*t;
                b1.vely-=Jy*t;
            };
            {
                var t=(b2.imass);
                {
                    #if NAPE_ASSERT var res={
                        !({
                            #if flash9 var ret:Bool=untyped __global__["isNaN"](t);
                            ret;
                            #else Math.isNaN(t);
                            #end
                        });
                    };
                    if(!res)throw "assert("+"!assert_isNaN(t)"+") :: "+("vec_addeq(in a: "+"b2.vel"+",in b: "+"J"+",in s: "+"b2.imass"+")");
                    #end
                };
                b2.velx+=Jx*t;
                b2.vely+=Jy*t;
            };
            b1.angvel-=((Jy*a1relx-Jx*a1rely)+Jz)*b1.iinertia;
            b2.angvel+=((Jy*a2relx-Jx*a2rely)+Jz)*b2.iinertia;
        };
        return false;
    }
}
